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Controllability  of  Mobile  Robot,  with  Kinematic 

Constraints 

Jerome  Barraquand  and  Jean-Claude  Latombe 

Robotics  Laboratory 
Computer  Science  Department 
Stanford  University 

Abstract:  We  address  the  contnllabUity  problem  for  robot  systenu  subjects 
kinemalie  constmints  on  the  velocity  and  its  application  to  path  planning.  We 
show  that  the  well-known  Contwllability  Rank  Condition  Theorem  u  applicable 
to  these  systems  when  there  are  inequality  constraints  on  the  velomty  m  addi¬ 
tion  to  equality  constraints,  and/or  when  the  constrainU  are  non-hnear  instep 
of  linear.  This  allows  us  to  infer  a  whole  set  of  new  results  on  the 
bility  of  robotic  systems  subject  to  non-integrable  kinematic  constraints  (aRkd 
nonholonomic  systems).  A  ear  with  Kmited  steering  angle  w  one  example  of 
such  a  system.  For  example,  we  show  that: 

1)  An  n-body  ear  system,  which  consists  of  a  car  toxeing  n  -  1  trailers,  is 
controllable  forn<A  even  if  the  steering  angle  is  limited. 

2)  An  n-body  car  (n  <  A)  that  can  only  turn  left  is  stUl  maneuverable  on  the 
right 

S)  If  there  is  a  path  for  an  n-body  car  system  (n  <  A)  with  limited  steenng  angle 
in  a  given  environment,  then  there  is  another  path  that  uses  only  the  extremal 
values  of  the  steering  angle.  We  conjecture  that  these  resulU  are  true  for  all  n. 
However,  we  have  only  been  able  to  prove  them  for  n  <  A. 

The  same  kind  of  results  as  above  can  also  be  obtained  for  any  nonholonomic 
system  additionally  subject  to  inequality  constraints  on  the  velocity. 

We  present  experiments  with  simulated  nonholonomic  systems  that  Ulustrate 
these  results.  These  experiments  were  conducted  by  using  a  general  path  planner 
previously  described  in  [Barraquand  and  Latombe  89b]. 
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Introduction 


In  this  paper  we  address  the  controllability  problem  for  robot  systems  in  the  pres¬ 
ence  of  kinematic  constrsdnts  on  the  velocity  and  its  application  to  path  planning. 
Kinematic  constraints  on  the  velocity  can  be  divided  into  two  maJn  classes: 

-Equality  constraints,  which  arc  generally  caused  by  one  or  several  rolling  contacts 
between  rigid  bodies  and  express  the  fact  that  the  relative  velocity  of  two  points  in 
contact  is  zero.  When  these  constraints  arc  non-integrablc,  they  make  the  dimen¬ 
sion  of  the  space  of  achievable  velocities  smaller  than  the  dimension  of  the  robot’s 
configuration  space.  They  arc  called  nonholonomic  equality  constraints.  We  call  non- 
holonomic  robot  a  robot  whose  motions  arc  constrsdned  by  nonholonomic  constraints. 

-Inequality  constraints,  which  arc  generally  caused  by  mechanical  stops  of  the  robot 
kinematics.  Unlike  equality  constraints,  they  do  not  reduce  the  dimension  of  the 
space  of  achievable  velocities.  Instead,  they  restrict  locally  the  authorized  directions 
for  the  velocity  to  a  subset  of  the  tangent  space. 

A  car  is  a  typical  example  of  a  nonholonomic  mechanical  system.  In  the  absence 
of  obstacles,  it  can  attain  any  position  in  the  plane  with  any  orientation.  Hence, 
the  configuration  space  is  three-dimensional.  However,  assuming  no  slipping  of  the 
wheels  on  the  ground,  the  velocity  of  the  midpoint  between  the  two  rear  wheels  of  the 
car  is  always  tangent  to  the  car  orientation.  The  space  of  achievable  velocities  at  any 
configuration  is  thus  two-dimensional.  This  corresponds  to  an  equality  constraint 
on  the  velocity.  If  in  addition  the  steering  angle  of  the  front  wheels  is  limited  by 
a  mechanical  stop,  the  space  of  achievable  velocities  at  any  configuration  is  further 
restricted  to  a  two-dimensional  cone  around  the  neutral  position.  This  corresponds 
to  an  inequality  constraint  on  the  velocity. 

Collision-free  path  planning  consists  of  constructing  a  path  in  the  free  subset  of  the 
configuration  space  -  i.e.  the  set  of  configurations  where  the  robot  has  no  contact 
or  intersection  with  the  obstacles  -  between  two  input  configurations.  Kinematic 
constraints  require  that  the  tangent  to  the  path  at  any  configuration  be  within  the 
subspace  of  velocities  selected  by  the  constraints.  A  collision-free  path  for  a  non¬ 
holonomic  robot  typicaJly  has  to  include  “maneuvers”,  i.e.  backing-up  configurations 
where  the  robot  stops  and  changes  the  sign  of  the  velocity  (think,  for  example,  of 
the  parallel  parking  of  a  car  along  a  sidewalk).  Finding  a  feasible  path  between  two 
configurations  is  one  difficult  problem.  Another  one  is  to  minimize  the  number  of 
maneuvers,  or  at  least  to  keep  it  reasonable,  whenever  possible. 

The  first  part  of  this  paper  is  a  generalization  of  the  mathematical  analysis  of  non¬ 
holonomic  constraints  developed  in  [Barraquand  and  Latombe  89b].  Using  standard 
results  in  nonlinear  control  theory  (namely,  the  Controllability  Rank  Condition  The¬ 
orem  for  Non-Linear  Systems),  we  state  a  general  result  applicable  to  robot  systems 
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,ubi«*  to  «iujity  and/or  in«ioaIity  conatrdnt.  on  the  r«locily.  W.  then  inter  new 
results  on  the  controllability  of  robotic  systems. 

The.«ondparto,th.p™«-J^^^ 
which  we  name  the  n-body  car  sysiem.  /\  i*  j  , 

r,“^rJ::^''i!t:^I:7td2.ibte  ponuon.  ot  the  .teenn*  wheel.  The  following  three 
statements  are  direct  consequences  of  this  result: 

1)  An  n-body  car  (n  <  4)  is  controllable  even  if  the  steering  angle  is  limited. 

2)  An  n-body  car  (n  <  4)  that  can  only  turn  left  is  still  maneuverable  on  the  right. 

31  K  there  exists  a  path  for  an  n-body  car  (n  <  4)  with  limited  steering  angle  in  a 
given  environment,  then  there  exists  another  path  that  uses  only  the  extremal  values 

of  the  steering  angle. 

W.  conjure  that  th«.  r«»lt.  are  tru.  for  aU  n.  Howerrer,  w.  ha-«  only  been  able 
to  prove  them  for  n  <  4. 

Fioallv,  in  the  third  part  of  the  pap«,  we  pr«^t 
elated  nonbolonomic  .yatema  that  iUestrate  the  alewe  roelta. 

^riment.  were  conducted  by  using  a  path  planner  previously  described  in 

(Barraquand  and  Latombe  89bl.  The  experiments  *“  ^ 

ried  o^t  with  a  version  of  the  planner  specific^ly  d^ij^ed  to  ” 

of  maneuvers.  In  this  version,  the  planner  applies  a  bmte 

of  performing  a  dynamic  programming  search  in  the  discretized  configuration  sp^e 
with  the^umber  of  maneLrs  as  the  cost  function.  Despite  its  ^nceptual  simphc- 
ity,  the  planner  is  relatively  fast  with  reasonable,  but  non  ^ 

knowledge,  this  is  the  first  implemented,  planner  capable  of  finding 
with  qu«i-minimal  number  of  maneuvers  (at  the  resolution  of  the  configuration  space 

representation)  for  nonholonomic  robots. 

Pc«ibl.  .pplic»li«n.  of  the  planner  includ.  ..vigation  of 
mrt«l  Peking  of  p«soo  J  cara  imd  truck.,  autonomous  na..gat,on  of 
in  airport  fac‘iti«,  automatic  pluming  of  th.  mouemenU  «'  "1“^”'""  * 
tion  L,  and  computer-aided  design  of  access  port,  for  truck,  m  industrial  and  com 

mercial  facilities. 


2  Relation  to  Other  Work 


Research  on  collision-free  path  plajining  has  been  very  active  during  the  past  ten 
years  (e.g.  see  [Latombe  90]).  Today,  the  mathematical  and  computational  struc¬ 
tures  of  path  planning  for  holonomic  robots  are  reasonably  well-understood.  Prac¬ 
tical  planners  have  also  been  implemented  in  more  or  less  specific  cases  (e.g., 
[Brooks  and  Lozano-Perez  83]  (Gouzenes  84]  [Laugier  and  Germain  85]  [Faverjon  86] 
[Faverjon  and  Toumassoud  87]  [Lozano-Perez  87]  [Barraquand  and  Latombe  89a]). 
However,  path  planning  with  nonholonomic  constraints  has  attracted  much  less  in¬ 
terest  30  far. 

The  problem  was  first  introduced  by  Laumund  [Laumond  86],  who  proved  that  the 
single-body  car,  i.e.  a  car  without  a  trailer,  is  controllable.  Laumond’s  construc¬ 
tive  proof  can  be  used  to  design  an  actual  two-phase  planner.  In  the  first  phase, 
the  planner  generates  a  free  path  by  ignoring  the  nonholonomic  construnts.  In  the 
second  phase,  ?t  transforms  this  path  into  a  topologically  equivalent  free  path  that 
satisfies  the  nonholonomic  constraints.  The  transformation  is  done  by  introducing 
as  many  backing-up  maneuvers  as  necessary.  However,  the  number  of  maneuvers 
generated  by  this  planner  is  unbounded  (see  [Laumond  et  al.  88]),  even  when  there 
exist  collision-free  paths  with  no  or  few  maneuvers  that  satisfy  the  nonholonomic 
constraint.  Building  on  his  previous  work,  Laumond  sketched  an  algorithm  for  plan¬ 
ning  smooth  —  i.e.  maneuver-free  —  collision-free  paths  of  a  nonholonomic  circular 
robot  whose  turning  radius  is  lower-bounded  [Laumond  87].  But  this  algorithm  fails 
whenever  all  free  paths  require  one  or  more  maneuvers. 

Another  planning  approach  has  been  specifically  developed  and  implemented  for 
one-body  car  robots.  It  consists  of  planning  the  motion  of  the  robot  in  a  network 
of  corridors"  [Toumassoud  and  Jehl  88]  or  “lanes”  [Wilfong  88]  that  are  extracted 
from  the  workspace  in  a  first  phase  of  processing.  Local  planning  techniques  are  used 
to  generate  turns  for  transferring  the  robot  from  a  corridor  (or  lane)  to  another  at  a 

crossroad  of  the  network.  The  difficulty  of  the  approach  is  that  for  most  workspaces 
one  cannot  define  an  intrinsic  set  of  corridors  (or  lanes).  In  addition,  the  various  turns 
that  have  to  be  generated  along  a  path  usually  interact,  and  local  planning  techniques 
may  result  in  quite  inefficient  paths. 

The  motion  of  a  point  along  paths  having  lower-bounded  curvature  radius  has  been 
investigated  in  [Fortune  and  Wilfong  88]  and  [Jacobs  and  Canny  89].  Fortune  and 
Wilfong  proposed  an  exact  algorithm  for  deciding  whether  there  exists  a  feasible  path 
between  two  configurations  among  polygonal  obstacles.  Jacobs  and  Canny  adapted 
one  of  the  results  presented  in  [Fortune  and  Wilfong  88]  and  described  an  imple¬ 
mented  planner.  This  planner  consists  of  discretizing  the  boundary  of  the  obstacles 
and  connecting  the  points  resulting  from  this  discretization  by  paths  of  standard 
shapes,  called  “jumps”,  which  are  made  of  circular  arcs  and  straight  segments. 


(Li  and  Canny  89]  and  [Laumond  and  Simeon  89]  pointed  out  that  results  in 
nonlinear  control  theory  where  transposable  in  order  to  characterize  the  con¬ 
trollability  of  robots  subject  to  linear  equality  constraints  on  the  velocity. 
[Laumond  and  Simeon  89]  and  [Barraquand  and  Latombe  89b]  used  these  results  to 
prove  the  controllability  of  the  two-body  car  system.  However,  both  proofs  con¬ 
sider  only  the  linear  equality  constraint  on  the  velocity  imposed  by  the  rolling  of  the 
wheels  of  the  ground.  They  are  not  applicable  when  there  is  an  additional  inequality 
constraint  imposed  on  the  steering  angle  by  the  mechanical  stops. 

Our  contributions  to  robot  motion  planning  with  nonholonomic  constr^nts  reported 
in  this  paper  are  the  following: 

1)  We  show  that  a  careful  instantiation  of  the  Controllability  Rank  Condition  The-  . 

orcm  subsumes  and  generalizes  most  previous  results  on  the  controllability  of 
nonholonomic  robots. 

2)  We  apply  this  result  to  the  one-body,  two-body,  and  three-body  car  systems  and 

we  derive  new  formal  results  related  to  the  controllability  of  these  systems. 

3)  We  describe  a  planner  that  has  been  able  to  solve  more  complex  planning  problems 

with  nonholonomic  robots  than  any  previous  implemented  planner, 

3  Non-linear  Control  Systems 

Throughout  this  paper,  we  regard  a  nonholonomic  robot  as  a  nonlinear 
system.  In  this  section  we  recall  the  definition  of  some  basic  concepts 
trollability  theory  and  we  review  importamt  results  related  to  non-linear 
([Hermann  and  Krener  77]). 

3.1  Definition  of  Controllability 

Let  us  consider  a  control  system  of  the  form: 

7  =  /(7.«)  (1) 

where  u  €  a  measurable  subset  of  1?^,  9  €  C,  a  connected  manifold  of  dimension  n. 

/  is  smooth  as  a  function  of  q.  fl  represents  the  control  space  of  the  system,  i.e.  the 
set  of  admissible  control  values.  C  represents  the  state  space^  or  configuration  space., 
i.e.  the  set  of  distinguishable  states  that  the  system  may  take  at  any  given  time. 

Given  a  subset  U  C  C,  the  configuration  qi  £  U  \s  said  to  be  U-accessible  from  qo  €  U 
if  there  exists  a  bounded  measurable  control  such  that  the  solution 


control 
in  con- 
control 


q{t)  of  the  system  (1)  satisfies:  q{to)  =  9o>?(fi)  =  9i  ?(0  €  f/,Vf  €  (<o»ti].  We 
write  qiAuqo.  The  set  of  points  t/-accessible  from  qo  will  be  denoted  by  Af;{qo). 

The  system  (1)  is  controllable  at  qo  iff  A:(9o)  =  C.  If  this  is  true  for  any  point  then 
the  system  is  controllable.  This  simply  means  that  any  point  is  C-accessible  from  any 
other  point.  This  global  notion  of  controllability  is  not  well-suited  to  mathematical 
characterizations.  A  much  more  useful  concept  is  the  local  notion  of  controllability 
defined  below. 

The  system  (1)  is  locally  controllable  at  qo  iff  for  every  neighborhood  U  of  qo,  Au(qo) 
is  also  a  neighborhood  of  qo.  It  is  locally  controllable  iff  this  is  true  for  every  qo- 

Accessibility  is  a  reflexive  and  transitive  relation,  but  not  necessarily  symmetric. 
The  symmetric  closure  of  this  relation  is  called  weak  accessibility,  qf  is  weakly  U- 
accessible  from  q  iff  there  exists  a  sequence  qo,..‘y9k  such  that  q  =  qoy  ^  ~  9fci  and 
either  qiAuqi-i  or  qi.xAvq,  for  every  t  €  (1,  k].  We  write  qWAu(f. 

The  system  (1)  is  weakly  controllable  at  qo  iff  WA{qo)  =  C.  If  this  is  the  case,  the 
system  is  necessarily  weakly  controllable  at  any  other  point,  because  weak  accessibility 
is  an  equivalence  relation,  and  it  is  weakly  controllable.  Again,  weak  controllability  is 
a  global  concept  that  has  a  useful  local  equivalent. 

System  (1)  is  locally  weakly  controllable  at  qo  if  for  every  neighborhood  U  of  qo, 
WAu{,q(^  is  a  neighborhood  of  qo.  It  is  locally  weakly  controllable  if  this  is  true  at 
every  qo. 

Clearly,  local  (weak)  controllability  implies  (weak)  controllability.  Furthermore,  for 
symmetric  systems,  (local)  weak  controllability  is  equivalent  to  (local)  controllability. 
Therefore,  for  a  symmetric  system,  local  weak  controllability  implies  controllability. 


3.2  Frobenius  Theorem 

Consider  the  set  X{C)  of  smooth  vector  fields  on  C.  Using  (1),  each  constant  control 
u  €  n  defines  a  vector  field  X„  =  /(.,  u)  on  C.  We  denote  by  F  the  set  of  all  the 
vector  fields  corresponding  to  the  admissible  values  of  the  control: 

F  =  {xe  x(C)|3u  esi,x  =  /(.,u)} 

We  define  an  internal  operation  on  A' (C)  that  defines  a  Lie  algebra  structure. 

Let  {X,  Y)  be  any  pair  of  vector  fields.  Given  any  configuration  q,  let  us  consider  a 
path  starting  at  q  obtained  by  concatenating  four  consecutive  paths: 

-  the  first  path  follows  the  flow  of  X  during  St; 

-  the  second  path  follows  the  flow  of  Y  during  St; 
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-  the  third  path  follows  the  flow  of  -X  during  SV, 

-  the  fourth  path  follows  the  flow  of  -Y  during 

We  denote  by  q'  the  configuration  reached  at  the  end  of  these  four  paths.  A  straight¬ 
forward  Taylor  expansion  shows  that: 

The  expression  dY-X-dX-Y  determines  a  new  vector  field  which  is  commonly 
denoted  by  and  called  Lit  bracket  of  A'  and  y. 

By  definition,  the  Control  Lie  Algebra  assodatod  with  F,  denoted  by  CLA{F),  is 
the  smallest  subalgebra  of  X{C)  which  contains  F.  Stated  otherwise,  CLA(F)  is  the 
subspacc  of  X{C)  generated  by  all  linear  combinations  of  vector  fields  in  F  and  all 
their  Lie  brackets  recursively  computed. 

For  each  90  €  C,  let  CLA{F){qQ)  denote  the  subspace  of  tangent  vectors  spanned  by 
the  vector  fields  of  CLA{F)  at  qo. 

A  connected  submanifold  C  of  C  is  an  integral  submanifold  of  CLA{F)  if  at  each 
q  &  C*  the  tangent  space  to  C  is  contained  in  CLA{F){qY  C  is  a  maximal  integral 
submanifold  of  CLA{F)  if  it  is  not  properly  included  in  any  other  integral  manifold. 

The  Frobenius  integrability  theorem  can  be  stated  as  follows: 

Theorem  1  (IVobenius  Integrability  Theorem) 

If  the  dimension  ofCLA{F){q)  has  a  constant  value  k  for  every  q  €  C,  there  exists 
a  partition  of  C  into  maximal  integral  submanifolds  of  CLAiF)  all  of  dimension  k. 


3.3  Controllability  Rank  Condition 

The  system  (1)  is  said  to  satisfies  the  Controllability  Rank  Condition  at  qo  if  the 
dimension  of  CLA{F){qo)  is  n.  If  this  is  true  for  every  qo  then  it  satisfies  the  Con- 
trollability  Rank  Condition. 

The  following  results,  based  on  the  work  of  [Chow  39],  were  derived  by 
[Hermann  631,  [Haynes  and  Hermes  70),  and  working  independently  by  [Lobry  70], 
[Sussmann  and  Jurdjevic  72],  [Krener  74]. 

Theorem  2  (The  Controllability  Rank  Condition  Theorem;  Sufficient  Con¬ 
dition) 

If  the  system  (1 )  satisfies  the  Controllability  Rank  Condition  at  qo,  then  it  is  locally 
weakly  controllable  at  qo- 


Theorems  (The  Controllability  Rank  Condition  Theorem;  Necessary 
Condition) 

If  the  system  (1)  is  locally  weakly  controllable,  then  the  Controllability  Rank  Condi¬ 
tion  is  satisfied  on  an  open  dense  subset  of  C. 


In  particular,  if  we  are  only  interested  in  symmetric  systems  for  which  the  dimension 
of  CLA{F){q)  does  not  depend  on  q  we  can  infer  that  local  controllability  (which 
implies  controllability)  is  equivalent  to  the  Controllability  Rank  Condition. 

Another  presentation  of  the  Controllability  Rank  Condition  Theorem  based  on  the 
concept  of  distribution  can  be  found  in  [Isidori  85].  Its  relation  to  nonholonomic  sys¬ 
tems  is  analyzed  in  in  [Li  and  Canny  89]  and  [Barraquand  and  Latombe  89b].  The 
formulation  developed  here  allows  to  deal  with  non-linear  equality/inequality  con- 
strunts,  whereas  the  alternative  presentation  using  distributions  is  limited  to  linear 
equality  constraints. 


4  Nonholonomic  Constraints 

4  •  1  Terminology 

We  denote  the  robot  by  A  and  its  workspace  by  W.  A  configuration  of  ^  is  a 
specification  of  the  position  of  every  point  in  A  with  respect  to  a  Cartesian  frame 
embedded  in  W.  The  configuration  space  of  A  is  the  space,  denoted  by  C,  of  all  the 
possible  configurations  of  A,  The  configuration  space  of  a  mechanicaJ  system  made  of 
rigid  bodies  is  a  smooth  manifold  [Arnold  78].  F^or  instance,  the  configuration  space 
of  a  twodimensional  rigid  body  translating  and  rotating  in  W  =  is  C  =  R^  x  S', 
where  S'  denotes  the  unit  circle.  In  virtually  any  practical  situation,  the  range  of 
positions  reachable  by  the  robot’s  bodies  can  be  bounded,  making  C  into  a  compact 
manifold.  Let  n  be  the  dimension  of  C.  A  configuration  q  is  represented  as  a  list 
(71, . . .  ,qfn)  of  ^  generalized  coordinates. 

Now,  suppose  that  a  scalar  constraint  of  the  form: 


F(g,<)  =  0  (2) 

with  q  €C  and  t  denoting  time,  applies  to  the  motion  of  A.  Let  us  further  assume 
that  F  is  smooth  with  non-zero  derivative.  Then,  in  theory,  one  could  use  the  equation 
to  solve  for  one  of  the  generalized  coordinates  in  terms  of  the  other  coordinates  and 
time.  Thus,  equation  (2)  defines  a  (n  —  l)-dimensional  submanifold  of  C.  This 
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submanifold  is  in  fact  tLc  actual  configuration  spaced  of  A  and  the  n  1  remaining 
coordinates  its  actual  generalized  coordinates,  ^nstraint  (1)  is  caU^  a  Aolonom.c 
equality  constraint.  More  generally,  there  -r.ay  be  k  cons  ram  s  of  the  form  (2)  If 
they  are  independent  -  i.e.  their  Jacobian  matrix  has  full  rank  -  they  determine  a 
(„  1  Jb)-dimensional  submanifold  of  C,  which  is  the  actual  configuration  space  of  A. 

A  constraint  of  the  form: 

F(«,0<0  Of  F(q,t)i,0 

where  F  is  smooth  with  r'i  .«.o  derivative,  is  an  inequality  constrant.  It  ^picJly 
acts  as  a  mechanical  stop  or  an  obstacle.  It  simply  determines  a  subset  of  C  having 

the  same  dimension  as  C. 

Constraint  (2)  is  only  a  kinematic  constraint  of  one  sort.  Another  one  is  a  scalar 
constraint  of  the  form: 


G(q,q,i)  =  0 


(3) 


with  q  e  TJC),  the  tanqent  space  of  C  at  q.  The  pair  (?,?)  belongs  to  TB{C),  the 
tangeni  bridle  associated  with  the  manifold  C  The  tangent 
space  of  the  velocities  of  A.  The  tangent  bundle  is  also  called  the  phase  space 
in  Physics  and  the  “state  space”  in  control  theory.  The  tangent  space  of  a  smooth 
manifold  is  a  vector  space  having  the  same  dimension  m  the  manifold.  Hence,  r,(  ) 
has  dimension  .%  for  every  ?  €  C.  The  tangent  bundle  TB{C)  is  a  smooth  manifold  o 

dimension  2n. 

A  kinematic  constraint  of  the  form  (3)  is  holonomic  if  it  is  integrable,  i.e.  qc^he 
eliminated  and  the  equation  (3)  rewritten  in  the  form  (2).  Otherwise,  the  ~;«tr^nt 
is  called  a  nonholonomic  equality  constraint.  As  we  will  see  Mow,  a  nonholonomic 
equality  constraint  restricts  the  space  of  velocities  achievable  by  .4  at  any  ronfigura- 
tion  0  to  a  (n  -  l)-dimensional  linear  subspace  of  T,{C)  without  affMing  the  dimen¬ 
sion  of  the  configuration  space.  If  there  are  k  independent  nonholonomic  equality 
constraints  of  the  form  (3),  the  space  of  achievable  velocities  is  a  subspace  of  i,(C) 
of  dimension  n  —  fc. 

A  nonholonomic  equality  constraint  is  generally  caused  by  a  rolling  contact  betwwn 
two  rigid  bodies.  It  expresses  the  fact  that  the  relative  velocity  of  the  two  points 
of  contact  is  zero.  When  the  motion  in  contact  combines  rolling  and  sliding,  the 
expression  depends  on  the  friction  coefficient  of  the  two  bodies,  and  hence  is  nonlinear. 
When  there  is  no  sliding,  the  nonholonomic  constraint  is  linear  in  q.  The  second  case, 

Ilf  constraint  (2)  depends  on  t.  ^’s  configuration  space  is  time-dependent,  J 

independent.  Many  usual  holonomic  constraints  -  e.g..  the  prismatic  and  revolute  jointe  of  a  ma¬ 
nipulator  arm  -  are  time-independent. 
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though  less  general  than  the  first,  is  much  simpler  and  quite  widespread  in  practice. 
For  instance,  in  the  car  example,  this  corresponds  to  assuming  no  slipping  of  the 
wheels  on  the  ground. 

A  constriunt  of  the  form: 

GU.9.0<0  or  <^(9.9.0  ^0 

is  a  kinematic  inequality  constrant.  It  restricts  the  set  of  achievable  velocities  at  any 
configuration  g  to  a  subset  of  T^{C)  having  the  same  dimension  as  T,(C).  A  constraint 
bounding  the  steering  angle  of  a  car  is  a  typical  kinematic  inequality  constraint. 

When  dealing  with  constraints  of  the  form  (3),  two  important  questions  arise: 

-  Are  they  integrable? 

-  If  they  are  not  integrable,  do  they  restrict  the  range  of  achievable  configurations? 

We  investigate  these  questions  in  the  next  two  subsections.  First,  we  show  that 
under  very  general  conditions  the  concept  of  kinematic  constraint  on  a  mechanical 
system  is  dual  and  equivalent  to  the  concept  of  control  system  as  defined  in  (1). 
This  equivalence  enables  us  to  use  results  from  nonlinear  control  theory  to  answer 
our  questions.  Using  the  Frobenius  Integrability  Theorem,  we  give  a  necessary  and 
sufficient  condition  of  holonomy  (and  non-holonomy)  for  equality  constraints  of  the 
form  (3).  Then,  using  the  Controllability  Rank  Condition  Theorem  we  analyze  the 
second  question.  We  state  a  necessary  and  sufficient  condition  under  which  kinematic 
constraints,  whether  they  are  linear  or  nonlinear,  equality  or  inequality,  have  no  effect 
on  the  range  of  achievable  configurations. 

For  simplicity,  in  the  rest  of  the  paper,  we  will  assume  that  the  kinematic  constraints 
do  not  depend  on  time.  However,  all  the  results  remain  valid  when  constraints  are 
time-dependent. 

4.2  Kinematic  Constraints  and  Control  Systems 

Let  us  consider  a  set  of  it  <  n  independant  kinematic  constraints  of  the  form  (3): 

^»(9»9)  =  (<Ji(9»9).---»^k(9'9^)  =  (0,...,0) 

For  each  q,  G,  =  G{q, .)  defines  a  function  from  T,(C)  to  R*.  As  the  k  constraints 
are  independent,  the  Jacobian  of  this  function  has  full  rank.  The  subset  of  the  tangent 
space  verifying  the  kinematic  constraints  is  simply  ...  ,0).  According  to  the 

Generalized  Implicit  Function  Theorem  (also  called  Constant  Rank  Theorem),  this 
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subspacc  is  a  submanifold  of  7'y(C)  of  dimension  n—k.  Let  us  denote  a  local  coordiriate 
chart  of  this  manifold  by  u  =  (ufc+i,...,Un),  and  define  /,  =  «"*.  We  obt^n  the 
following  relation: 

9  =  /«{«)  =  /(9.“) 

Under  the  additional  assumption  that  /  is  smooth  as  a  function  of  q,  this  relation 
locally  defines  a  non-linear  control  system  with  n  —  k  controls  , «n)-  Assume 

that  we  impose  inequality  constraints  in  addition  to  equality  constraints.  These  new 
constraints  are  transformed  into  inequalities  on  the  controls  by  means  of  the  inverse 
of  the  chart  u.  They  define  the  shape  of  the  set  f)  of  admissible  controls. 

Reciprocally,  if  we  consider  any  control  system  of  the  type  (1)  such  that  f{q, .)  =  /, 
has  full  rank  as  a  function  of  the  control  u  =  {uk+\f>,Vn)t  then  we  can  apply 
again  the  Constant  Rani^  Theorem  for  every  q  and  obtain  a  local  atlas  chart  G,  = 
(GJ,...,G;)  verifying: 

CrM^))  =  <7,(9)  =  G‘(9,9)  =  0,Vi  €  [1,*] 

and 

G;(/,(«))  =  <7,(9)  =  e[k  +  l.n] 

The  first  k  equalities  given  above  precisely  define  k  independent  kinematic  constraints. 
Furthermore,  the  inequalities  on  the  controls  that  define  the  shape  of  the  set  il  are 
transformed  into  inequalities  on  the  velocity  by  means  of  the  Gj,t  €  (if  -b  l,n). 

Therefore,  in  general,  a  mechanical  system  subject  to  a  set  of  k  independent  kine¬ 
matic  construnts  is  locally  equivalent  to  a  control  system  contsdning  n  —  k  controls 
for  which  the  function  /  has  full  rank  in  u.  Furthermore,  any  additional  inequality 
constrsunts  on  the  velocities  are  equivalent  to  inequality  constrsunts  on  the  controls. 

In  some  practical  cases,  this  local  equivalence  between  kinematically  constrained 
robots  and  control  systems  can  be  glob2dized.  In  particular,  when  there  are  only  pure 
rolling  constraints  on  the  system,  G  is  linear  in  9  and  the  global  equivalence  can  be 
formally  proven  (see  (Barraquand  and  Latombe  89b]). 

4.3  Nonholonomy  and  Controllability 

Let  us  consider  a  mechanical  system  subject  to  a  set  of  k  independent  kinematic 
equality  constraints  of  the  form  (3).  To  answer  the  integrability  question,  we  first 
compute  as  indicated  above  the  equivalent  control  system,  that  is  the  function  /(9,u). 

We  can  characterize  the  integrability  of  the  constraints  using  Frobenius  Theorem. 
For  each  configuration  9  the  dimension  r  of  CLA{F){q)  is  clearly  greater  than  or 
equal  to  n  —  k. 
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If  r  takes  values  greater  than  n  -  ib,  then  the  maximal  integral  manifolds  have  a 
dimension  greater  than  n  —  ib,  and  the  system  is  non-integrable,  i.e.  nonholonomic. 

On  the  other  hand,  if  r  remains  equal  to  n  -  ib  at  every  q,  then  the  Frobenius 
Theorem  entails  that  the  maximal  integral  manifolds  of  the  system  have  dimension 
n  —  Jb.  Therefore,  the  configurations  of  the  system  always  stay  in  an  n  —  t  dimensional 
submanifold  of  C.  As  a  consequence,  the  velocities  always  belong  to  the  tangent 
space  of  this  submanifold,  which  is  precisely  CLA(F){q)^  But  these  same  velocities 
belong  to  a  n  —  ib  dimensional  submanifold  S  of  the  tangent  space  of  C  at  f  defined 
by  the  construnts.  Therefore,  S  is  necessarily  equal  to  CLA{F){q)y  hence  linear. 
This  implies  that  the  k  equality  constrsunts  are  linear  in  q.  This  result,  though 
intuitively  clear,  is  worth  being  outlined.  To  characterize  holonomic  constraints,  we 
can  therefore  limit  ourselves  to  those  that  are  linear  in  the  velocity  parameters.  In  that 
case,  we  can  replace  the  configuration  space  C  by  the  maximal  integral  submanifold 
passing  through  the  initial  configuration,  and  get  rid  of  the  constraints.  The  equations 
defining  this  submanifold  can  be  written  locally  in  the  form  (2),  i.e.  F{q)  =  0,  which 
is  the  integral  form  of  the  constraints  (3).  By  differentiating  this  last  equation  as  a 
function  of  time,  we  find  again  the  constraints  on  the  velocity: 

=  0 

which  gives  a  more  intuitive  explanation  of  the  fact  that  holonomic  constraints  are 
necessarily  linear. 

Proposition  1  (Non-holonomy  of  non-linear  constraints)  Kinematic  con- 
strciiits  that  are  properly  non-linear  as  functions  of  the  velocity  are  necessarily  non¬ 
holonomic. 

Proposition  2  (Characterization  of  Holonomy)  A  system  subject  to  k  indepen¬ 
dent  equality  constraints  of  the  form  (S)  is  holonomic  if  and  only  if  the  codimension 
n  ^  r  of  the  Control  Lie  Algebra  is  equal  to  the  number  k  of  const^ints.  In  such  a 
case,  the  kinematic  constraints  are  necessarily  linear  in  the  velocity  parameters. 

The  answer  to  the  controllability  question  for  robots  subject  to  kinematic  constraints 
is  a  direct  consequence  of  the  Controllability  Rank  Condition  Theorem.  As  outlined 
above,  given  k  independent  constraints,  we  consider  the  equivalent  control  system 
with  n  —  i  controls.  Then  we  analyze  the  dimension  r  of  the  Control  Lie  Alge¬ 
bra  by  recursively  computing  the  Lie  brackets  of  constant  control  vector  fields.  If 
this  number  r  is  constant  and  equal  to  n,  then  the  system  is  locally  weakly  control¬ 
lable.  Reciprocally,  if  the  system  is  locally  weakly  controllable,  then  r  is  equal  to 
n  on  an  open  dense  subset  of  C.  If  r  varies  on  C,  complex  phenomena  may  occur. 
The  study  of  these  phenomena  forms  the  basis  for  the  so-called  Catastrophe  Theory 
[Poston  and  Stewart  78], 
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Figure  1:  A  Two-Body  Car 

Proposition  3  (Characterization  of  Controllability)  A  system  subject  to  kine¬ 
matic  constraints  on  the  velocity  --  which  may  be  linear  or  non-linear,  equality  or 
inequality  —  is  locally  weakly  controllable  if  the  dimension  r  of  the  Control  Lie  Alge¬ 
bra  is  mcaimcl,  i,e.  equal  to  the  dimension  n  of  the  configuration  space. 


5  The  Multi-Body  Car  System 

Let  U8  consider  a  front-wheel-drive  four-wheel  car’.  We  model  the  car  by  a  two- 
dimensional  object  translating  and  rotating  in  the  plane.  The  configuration  space  of 
the  car  is  17  X  5*,  where  D  is  a  compact  domain  of  R’.  We  parameterize  the  car 
configuration  by  the  coordinates  Xi  and  Y\  of  the  midpoint  P\  between-the  two  rear 
wheels  and  the  angle  0\  between  the  x  axis  of  the  Cartesian  frame  einbedded  in  the 
plane  and  the  main  axis  of  the  car.  The  velocity  parameters  are  Xi,  Yi  and  Oi.  The 
control  parameters  of  the  car  are  the  velocity  w  €  R  of  the  midpoint  P©  between  the 
two  front  wheels  (if  u  >  0,  the  car  moves  for«;'aid)  and  the  steering  angle  ^  measuring 
the  orientation  of  the  velocity  of  Po  with  expect  to  the  miun  axis  of  the  car. 

Assuming  a  pure  rolling  contact  between  the  wheels  and  the  ground  -  i.e.  no  slipping 
—  the  velocity  of  Pi  is  always  along  the  main  axis  of  the  car.  Hence,  we  have: 

Xi  =  Acos^i  Yi  =  Asin^i. 

Eliminating  X,  we  get  the  following  kinematic  constraint  on  the  velocity: 

-  Xi  sin  $i  -1-  Vi  cos  =  0.  (4) 

*Our  presentation  can  easily  be  modified  to  treat  other  types  of  car-like  robots. 
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The  equivalent  control  system  is  easily  computed: 


Xi  =  vcos^cos^i 
Yi  =  vcos^sin^t 
LiOi  =  vsin^ 


As  w  can  take  both  positive  and  negative  values,  the  system  is  symmetric.  In  such  a 
case  weak  controllability  is  equivalent  to  controllability,  and  the  results  of  the  previous 
sections  are  applicable. 

The  multi*body  car  system  consists  of  adding  one  or  more  bodies  to  be  towed  by  the 
car.  For  example,  the  two-body  car  problem  consists  of  analyzing  the  behaviour  of 
the  mechanical  system  defined  by  a  car  towing  a  single  body.  In  this  problem,  there 
aic  two  kinematic  construnts:  the  velocity  of  the  midpoint  between  the  rear  wheels 
of  each  body  is  tangent  to  the  orientation  of  the  body. 

More  generally,  one  can  consider  the  n-body  car  system,  which  consists  of  a  car 
towing  n  —  1  bodies  serially  hooked  (e.g.,  a  luggage  carrier  in  an  urport).  Figure  1 
displays  a  schematic  model  of  such  a  system.  The  midpoint  between  the  rear  wheels 
of  the  first  body  (the  car)  is  denoted  by  Pi.  The  midpoint  between  the  rear  wheels 
of  the  body  is  denoted  by  P*.  We  therefore  have  r»  points  Pi,..., Pm,  whose 
coordinates  are  denoted  by  (Xi,yi),...,(XM,In).  The  orientation  of  the  k**  body 
with  respect  to  the  x  axis  of  the  Cartesian  frame  embedded  in  the  plane  is  denoted 
by  $k.  The  configuration  space  of  the  n-body  car  is  D  x  (S*)",  where  Z)  is  a  compact 
domain  of  R’.  We  parameterize  the  configuration  by  (Xi  ,Yi,0i,...,0n)-  The  velocity 
parameters  are  Xi,Ytt0i,...,$n’  The  control  parameters  are  the  same  as  for  the  car, 
that  is,  the  velocity  v  and  the  steering  angle 

'There  are  n  kinematic  constraints,  one  for  each  body.  To  establish  these  construnts, 
it  is  convenient  to  represent  the  points  Pi,..., Pm  in  the  complex  plane,  i.e.:  Pk  = 
Xk+iYk.Lk  denoting  the  length  of  the  k**  body,  we  can  write  the  geometric  construnt 
between  the  bodies  k  —  1  and  k  as: 


which  can  be  rewritten: 


Pk  =  Pk-i  -  Lktxp{i0k) 


Pk  =  Pt  Li  exp(i0i) 


The  kinematic  constraint  of  the  body  is: 

Pk  =  A*  exp(i0*) 


which  is  equivalent  to: 


9(exp(-itf*)P*)  =  0 
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where  S>(z)  denotes  the  imaginary  part  of  the  complex  number  z.  Combining  this 
characterization  with  the  derivative  of  equation  (6)  and  using  the  linearity  of  the  O 
operator,  we  obtdn  the  Jk‘*  kinematic  constr^nt: 

k 

—Xi  sin  +  ki  cos  ^  LiOf  cos{0i  —  0*)  =  0 

Is3 

In  particular,  we  obtain  for  k  =  1: 

-Xisin^j  +  yicostf,  =  0  (7) 


which  is  precisely  the  kinematic  constraint  (4)  of  the  car  problem. 

For  Jb  =  2,  we  get: 

—  ATisindj  +  licos^a  —  £3^2  =  0  (8) 

Equations  (7)  and  (8)  are  the  kinematic  constraints  of  the  two-body  car  problem. 
Similarly,  by  combining  P*  =  A*  exp{i0k)  with  the  derivative  of 


we  get 

and  by  induction: 


A*  =  cos(^*  —  ^jk-i)Ajk-i 


Pk  =  v  cos(^)  ■“  j  exp(*^*) 

Hence,  the  equivalent  control  system  of  the  n-body  car  is  composed  of  equations  (5) 
and  ^ 

=  vcos^  cos(^i  ”  sin(tf*_i  —  0k),  Vfc  €  (2,n] 

Let  X{y,4>)  denote  the  vector  field  corresponding  to  the  constant  control  (v,^).  We 
get: 

X{v,  if>)  =  cos(4^)  XCw,  0)  +  8in(^)  ^(t;,  ir/2) 

If  we  take  any  two  fields  corresponding  to  two  different  values  of  the  steering  angle 
^1  and  we  see  that  the  Control  Lie  Algebra  generated  by  A''(v,^i)  and  X{y,<t>2) 
is  the  same  as  the  one  generated  by  A'(v,0)  and  X(t;,T/2),  because  of  the  bilinearity 
of  the  Lie  Bracket  operation: 

[A>,^0.A>,^2)1  =  Sin(^,  -  ^2)(A(v,0),  A(i;,x/2)] 

Therefore,  the  dimension  of  the  Control  Lie  Algebra  of  the  n-body  car  sys¬ 
tem  is  not  affected  by  constraints  on  the  steering  angle.  It  has  been  shown  in 
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(Barraquand  and  Latombe  89b]  that  this  dimension  is  maximal  for  the  single- body 
and  the  two-body  car.  Here  we  generalize  this  result  to  a  three-body  system. 

The  constant  control  vector  field  space  is  generated  by  the  two  following  vector 
fields: 

Xi=  X(l.O)  =(  CM«„  .in*.,  0. 

X,=  IiX(l.l)  =(  0,  0.  1.  0,  0) 

whose  Lie  bracket  is: 

Xi  =  [XuXi]  =  (-sintfi,costfi,0,cos(5,  -  tfaVLa,  -  sin(tf,  -  tfa)sin(tfj  - 
We  next  compute: 

X4  =  i^[.^l,X3]  =  (0,0,0,l/ll,-cos(^2  -  $3)/L3) 

X,  =  cos((l,-(l2)(X„X4]-sin(tf,-tf,)(X3,.Y4] 

=  (0,0,0,  l/I|,-l/I§-cos(«2-«3)/(i^^)) 

Finally: 

det(X„X2,X3,X4,X5)  =  ^  0 

We  can  state: 

Proposition  4  A  tingle-body,  tvo-body,  or  three-body  ear  eystem  is  controllable 
whenever  there  are  at  least  two  different  admissible  positions  of  the  steering  wheel. 
In  particular: 

1)  >4  n-body  (n  <  4)  ear  system  is  controllable  even  if  the  steering  angle  is  limited. 

2)  A  n-body  (n  <  4)  car  system  that  can  only  turn  left  is  stUl  maneuverable  on  the 

right. 

3)  If  there  is  a  path  for  a  n-body  (n  <  4)  ear  system  with  limited  steering  angle  in  a 

given  environment,  then  there  is  another  path  that  uses  only  the  extremal  values 
of  the  steering  angle. 

These  statements  are  direct  consequences  of  the  fact  that  the  dimension  of  the 
Control  Lie  Algebra  is  not  affected  by  the  choice  of  the  steering  angles. 

The  controllability  problem  of  the  general  multi-body  car  system  (n  >  4)  could 
be  solved  by  computing  the  dimension  of  the  Control  Lie  Algebra  as  it  has  been 
done  above  for  n  <  4.  However,  the  symbolic  computations  of  Lie  Brackets  and 
determinants  are  non-trivial  in  higher  dimension  and  we  have  not  been  able  to  find  a 
general  recurrence  formula  for  the  determinant  so  far.  Investigations  are  under  way 
to  determine  if  symbolic  computation  programs  such  as  MACSYMA  can  facilitate 
this  task. 
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6  Planning  with  nonholonomic  Constraints 

Let  the  workspace  W  of  the  robot  A  be  populated  by  some  stationary  obstacles  B., 
t  =  These  obstacles  map  in  the  configuration  space  C  of  ^  to  regions  CB, 

called  C-ob$taclts  and  defined  by: 

CBi^{q£C/A{q)nBi^9} 

where  A{q)  denotes  the  region  of  W  occupied  by  ^  at  configuration  q.  The  subset 
Cfrtt  =  C\[fi^xCBi  is  called  free  space.  If  both  A  and  the  B.’s  arc  modelled  as 
closed  regions,  the  CB$*s  arc  closed;  C/rc«  is  an  open  subset  of  C,  hence  a  manifold  of 
dimension  n. 

Given  two  configurations  and  q2  in  Cfrf%  the  path  planning  problem  is  to  construct 
a  path  connecting  q\  to  ^2  lying  in  Cjr—t  L®-  *  continuous  map  r  :  a  €  (0, 1) 
r(a)  €  C/r««,  such  that  t(0)  =qi  and  r(l)  =q2.  In  the  presence  of  nonholonomic 
constradnts,  the  tangent  to  this  path,  must  lie  in  the  subset  of  the  tangent  space 
of  C  selected  by  the  constraints. 

We  have  implemented  a  general-purpose  path  planner  based  on  the  following  main 
ideas  [Barraquand  and  Latombe  89b]: 

The  configuration  space  is  decomposed  into  an  array  of  small  rectangloids  and  the 
planner  searches  a  directed  graph  whose  nodes  arc  these  rectangloids.  Two  rcctan- 
gloids  arc  adjacent  in  this  graph  if  there  is  a  feasible  free  path  between  a  configuration 
lying  in  the  first  rectangloid  and  a  configuration  lying  in  the  second.  The  arcs  of  the 
graph  are  constructed  by  discretizing  the  control  parameters  of  the  robot  and  in¬ 
tegrating  the  equations  of  motion.  The  graph  is  classically  searched  using  Bellman 
Optimality  Principle.  Our  implementation  uses  an  uninformed  A*  algorithm  (Dijk- 
stra^s  algorithm)  with  the  number  of  reversals  as  the  cost  function.  As  any  global 
motion  planning  technique,  the  approach  is  limited  to  robots  with  small  configuration 
spaces.  Indeed,  it  requires  time  O(r"logr)  and  space  0(r”),  where  r  is  the  size  of 
the  decomposition  along  each  axis  of  the  configuration  space. 

The  list  of  nodes  whose  successors  have  already  been  generated,  called  CLOSED, 
is  simply  represented  by  marking  the  corresponding  cells  in  a  large  n-dimensional 
array.  Hence,  the  access  time  to  CLOSED  is  constant.  The  list  of  nodes  whose 
successors  have  not  been  generated  yet,  called  OPEN,  is  represented  as  a  heap.  Every 
modification  and  access  to  OPEN  is  therefore  made  in  logarithmic  time.  These  two 
implementation  details  are  critical  for  the  efficiency  of  the  method. 

Given  a  configuration  the  planner  generates  at  most  six  successors  of  q  by  setting 
the  two  control  parameters  v  and  ^  to  the  six  values  in: 

{-Vo, Vo}  X  {-<f>max^0y+(f>max) 
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Figure  2:  A  Parallel  Parking  Maneuver 


and  integrating  the  velocity  parameters  of  the  robot  along  a  “short”  distance  using 
the  differential  equations  established  above.  By  short  distance,  we  mean  that  the 
integration  time  is  1  and  vo  is  equal  to  the  diameter  of  a  cell.  The  goal  is  to  avoid 
staying  in  the  same  cell  as  q,  while  not  moving  further  than  a  neighboring  cell. 
The  three  equations  of  a  single- body  car  can  be  integrated  analytically,  because  the 
integral  curves  are  simply  arcs  of  circles.  However,  the  remaining  n  —  3  equations  of  a 
general  n-body  car  have  to  be  integrated  numerically.  We  implemented  a  fourth-order 
Runge-Kutta  method  for  this  integration. 

This  discretization  of  the  control  parameters  derives  from  the  discussion  of  section  3. 
If  there  exists  a  feasible  free  path  between  two  given  configurations,  then  there  exists 
another  feasible  free  path  between  these  configurations  that  uses  only  the  extremal 
vaJues  of  the  steering  angle.  The  inclusion  of  ^  =  0  in  the  discretization  set  is  aumed 
at  allowing  the  robot  to  move  along  a  straight  path  and  hence  reducing  the  total 
length  of  the  generated  path. 

In  this  implementation,  collisions  are  checked  by  intersecting  the  robot  at  every 
attained  configuration  with  the  obstacles  in  the  workspace.  The  workspace  is  repre¬ 
sented  as  a  bitmap  anf  the  test  of  intersection  uses  a  divide-and-conquer  technique 
(see  [Barraquand  and  Latombe  89b]). 

Despite  its  conceptual  simplicity,  this  planner  outputs  quasi-optimal  solutions  to 
very  tricky  planning  problems  in  reasonable  amounts  of  time,  as  illustrated  by  the 
following  experimental  results. 

We  first  experimented  with  the  planner  using  a  simulated  car  with  various  values  of 
the  maximal  steering  angle  <^mar  and  several  workspace  arrangements. 


Figure  5:  Perking  a  Trailer 


Figure  6:  Trailer  Maneuvering  in  a  Cluttered  Workspace 


Figure  2  shows  an  example  of  the  parallel  parking  problem  with  a  very  limited 
steering  angle  (^m«*  =  30  degrees).  The  running  time  for  that  example  was  20 
seconds^. 

Figure  3  shows  an  example  with  backing-up  maneuvers  in  a  cluttered  workspace 
when  the  maximal  steering  angle  is  45  degrees.  The  running  time  was  about 
30  seconds.  Four  maneuvers  (i.e.  changes  of  the  sign  of  v)  were  necessary  in  this 
example. 

Figure  4  shows  an  example  of  maneuvering  for  a  car  that  can  only  turn  left.  The 
two  extremal  values  of  the  steering  angle  are  =  22.5  degrees  on  the  left  and 
=  45  degrees  on  the  left.  Seventeen  maneuvers  were  necessary  in  this  example. 

We  also  conducted  several  experiments  with  a  simulated  two-body  car. 

Figure  5  shows  an  example  where  the  two-body  car  has  to  be  parked  with  a  very 
limited  steering  angle  {4>mai  =  30  degrees).  The  running  time  was  2  minutes. 

Figure  6  shows  an  example  where  the  tridler  has  to  maneuver  in  a  cluttered 
workspace  with  a  maximal  steering  angle  equal  to  45  degrees.  The  running 
time  was  about  10  minutes. 


7  Conclusion 

In  this  paper,  we  presented  a  systematic  technique  for  studying  the  controllability 
of  robot  systems  subject  to  non-linear  and/or  inequality  constraints  on  the  velocity. 
In  particular,  it  has  been  shown  that  the  single-body,  two-body,  and  three-body  car 
systems  are  controllable  whenever  there  are  at  least  two  different  admissible  positions 
of  the  steering  wheel.  We  also  derived  the  differential  equations  of  motion  for  any 
multi-body  car  system.  However,  the  controllability  of  the  general  multi-body  car 
"ystem  has  not  been  proven,  although  we  conjecture  it. 

We  have  illustrated  these  results  using  a  general  path  planner  initially  presented 
in  [Barraquand  and  Latombe  89b].  This  pleuiner  generates  paths  for  multi-body  car 
systems  with  minimal  number  of  maneuvers  at  the  resolution  of  the  configuration 
space  discretization.  The  running  time  of  the  planner  is  quite  reasonable  for  single¬ 
body  and  two-body  cars,  even  in  very  cluttered  workspaces. 

However,  as  the  search  algorithm  requires  exponential  time  and  exponential  space  in 
the  number  of  bodies,  the  planner  is  not  practical  for  more  than  two  bodies.  Planning 
efficient  motions  for  cars  with  three  or  more  bodies  is  still  an  open  problem. 


^Our  planner  runs  on  a  DEC3100  workstation 
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